package astar;

import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
import java.util.PriorityQueue;
import java.util.Vector;

import nav.CrossingTopologyObjectImpl;
import nav.LinkTopologyObjectImpl;
import nav.TopologyImpl;

/**
 * A* algorithm implementation using the method design pattern.
 * 
 * @author Giuseppe Scrivano
 */
public class AStar {
	TopologyImpl topology;

	CrossingTopologyObjectImpl start;
	CrossingTopologyObjectImpl goal;

	// PriorityQueue<AStarNode> openList = new PriorityQueue<AStarNode>();
	HashMap<Integer, AStarNode> closedList = new HashMap<Integer, AStarNode>();
	ArrayList<AStarNode> openList = new ArrayList<AStarNode>();

	public AStar(TopologyImpl topology) {

		this.topology = topology;
	}

	public AStarNode run() {

		openList.add(new AStarNode(null, start));

		while (!openList.isEmpty()) {
			AStarNode current = getLeastF(openList);
			openList.remove(current);
			if (current.getCrossing().getCoordinate()
					.distanceTo(goal.getCoordinate()) == 0.0)
				return current;

			expand(current);

			closedList.put(current.getCrossing().getID(), current);

		}

		return null;
	}

	private static AStarNode getLeastF(List<AStarNode> l) 
	{
		AStarNode least = null;
		for (AStarNode n : l) {
			if ((least == null) || (n.getF() < least.getF())) {
				least = n;
			}
		}
		return least;
	}

	private void expand(AStarNode current) {

		Vector<AStarNode> successors = new Vector<AStarNode>();
		for (CrossingTopologyObjectImpl crossing : getSuccessors(current)) {

			AStarNode n = new AStarNode(current, crossing);
			n.g = calcG(n);
			n.h = calcH(n, goal);
			n.f = n.g + n.h;
			successors.add(n);
		}

		for (AStarNode successor_node : successors) {
			if (closedList.containsKey(successor_node.getCrossing().getID()))
				continue;

			 for(AStarNode node:openList){
			 if(node.getCrossing().getID()==successor_node.getCrossing().getID()){
				 //bereits in open-Liste
				 if(successor_node.getF()<node.getF()){
					 node.g = successor_node.getG();
					 node.f=node.g+node.h;
					 continue;
				 }
			 }
			 }
//			if (openList.contains(successor_node)
//					&& current.f >= successor_node.f) {
//
//				continue;
//			}
			openList.add(successor_node);
		}

	}

	private static double calcG(AStarNode n) // Bisheriger Weg:
	{
		AStarNode p = n.getParent(); // Bisheriger Weg des Vaterknotens + 1

		for (LinkTopologyObjectImpl link : n.getParent().getCrossing()
				.getLinks()) {
			if (link.crossingTo() == n.getCrossing().getID()) {
				return p.g + (double) link.length();
			}
		}

		return 0;
	}

	private static double calcH(AStarNode act, CrossingTopologyObjectImpl goal) // Heuristik
	{

		return act.getCrossing().getCoordinate()
				.distanceTo(goal.getCoordinate());

	}

	public void setStart(CrossingTopologyObjectImpl start) {
		this.start = start;
	}

	public void setGoal(CrossingTopologyObjectImpl goal) {
		this.goal = goal;
	}

	public ArrayList<CrossingTopologyObjectImpl> getSuccessors(AStarNode node) {
		ArrayList<CrossingTopologyObjectImpl> successors = new ArrayList<CrossingTopologyObjectImpl>();

		for (LinkTopologyObjectImpl link : node.getCrossing().getLinks()) {
			successors.add(topology.crossings.get(link.crossingTo()));
		}

		return successors;
	}

	public ArrayList<CrossingTopologyObjectImpl> getPath(
			CrossingTopologyObjectImpl start, CrossingTopologyObjectImpl goal) {
		this.start = start;
		this.goal = goal;
		AStarNode node = run();

		ArrayList<CrossingTopologyObjectImpl> reverse = new ArrayList<CrossingTopologyObjectImpl>();
		reverse.add(node.getCrossing());
		while (node.getParent() != null) {
			node = node.getParent();
			reverse.add(node.getCrossing());

		}
		ArrayList<CrossingTopologyObjectImpl> ret = new ArrayList<CrossingTopologyObjectImpl>();
		for (int i = reverse.size() - 1; i >= 0; i--) {
			ret.add(reverse.get(i));

		}
		return ret;
	}

}